#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     BR,            tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     BL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     FR,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     FL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Lift,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
  nMotorEncoder[BR] = 0;

  nMotorEncoderTarget[BR] = 960;
	nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);			// 960 ticks is one revolution on holonomic chassis
	motor[FL] = 50;
	motor[FR] = 50;
	motor[BL] = 50;
	motor[BR] = 50;

	while(nMotorRunState[BR] != runStateIdle)
	{
		nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);
		motor[FL] = motor[BR];
		motor[FR] = motor[BR];
		motor[BL] = motor[BR];
	}

	motor[FL] = 0;
	motor[FR] = 0;
	motor[BL] = 0;
	motor[BR] = 0;
}
